package ronin.ai;

import ronin.graphics.Vector2D;
import ronin.actor2d.*;
import ronin.graphics.GrimaceGamelet;
import ronin.util.MagicUtils;
import java.awt.geom.AffineTransform;
import java.util.*;
import ronin.ai.Path;
import java.awt.Point;

public class SteeringBehavior {
	
	AffineTransform matrix;
	
	public static final double MAX_STEERING_FORCE = 10.0;
	
	public static final int DECEL_SLOW = 3;
	public static final int DECEL_NORMAL = 2;
	public static final int DECEL_FAST = 1;
	
	// calculate types AND DEFAULT TYPE PRIORITIZED DITHERING
	public static final int CAL_TYPE_PD = 1000; //PRIORITIZED DITHERING
	public static final int	CAL_TYPE_WTS 		 = 1001; //WEIGHTED TRUNCATED SUM
	public static final int CAL_TYPE_WTS_PRIORITY = 1002; //WEIGHTED TRUNCATED SUM WITH PRIORITIZATION
	public int calculateType = CAL_TYPE_PD; 
	
	// do booleans
	private boolean doWallAvoidance, doObstacleAvoidance, doSeparation, doAlignment, doCohesion, doWander; 
	
	private SmartActor vehicle;
	private Path path;
	
	private double wanderRadius;
	private double wanderDistance;
	private double wanderJitter;
	private Vector2D wanderTarget;
	
	public double waypointSeekDistSq;
	
	private Vector2D steeringForce;
	
	public SteeringBehavior(SmartActor owner){
		vehicle = owner;
		path  = new Path();
		waypointSeekDistSq = 3;
		wanderRadius = owner.getWanderRadius();
		wanderJitter = owner.getWanderJitter();
		wanderDistance = owner.getWanderDistance();
		matrix = new AffineTransform();
		steeringForce = Vector2D.ZERO_VECTOR;
		
		doAlignment = false;
		doCohesion  = false;
		doObstacleAvoidance = false;
		doSeparation = false;
		doWallAvoidance = false;
		doWander = false;
	}
	
	/** Returns a force that directs an agent towards a target position. */
	public Vector2D seek(Vector2D targetPos) {
		Vector2D desiredVelocity = targetPos.minus(vehicle.getPos())
			.getNormalized().scaleBy(vehicle.getMaxSpeed());
		return desiredVelocity.minus(vehicle.getVel());
	}
	
	/** Is the opposite of seek, it returns a force that directs an agent away from a position. */
	public Vector2D flee(Vector2D targetPos) {
		Vector2D desiredVelocity = vehicle.getPos().minus(targetPos)
			.scaleBy(vehicle.getMaxSpeed());
		return desiredVelocity.minus(vehicle.getVel());
	}
	
	/** Arrive steers and agent in such a way that it decelerates onto 
	 * the target position. 
	 */
	public Vector2D arrive(Vector2D targetPos, int deceleration){
		Vector2D toTarget = targetPos.minus(vehicle.getPos());
		double dist = toTarget.length();
		
		if(dist > 0) 
		{
			double decelerationTweaker = 0.3;
			double speed = dist/(deceleration * decelerationTweaker);
			speed = Math.min(speed, vehicle.getMaxSpeed());
			Vector2D desiredVelocity = toTarget.scaleBy(speed).divide(dist);
			
			return desiredVelocity.minus(vehicle.getVel());
		}
		
		return new Vector2D.Double(0.0, 0.0);
	}
	
	/**  */
	public Vector2D persuit(SmartActor evader) {
		Vector2D toEvader = evader.getPos().minus(vehicle.getPos());
		
		double relativeHeading = vehicle.getHeading().dot(evader.getHeading());
		
		if((toEvader.dot(vehicle.getHeading()) > 0) &&
			(relativeHeading < 0.95))
			{
				return seek(evader.getPos());
			}
			
		double lookAheadTime = toEvader.length() / (vehicle.getMaxSpeed() 
			+ evader.getMaxSpeed());
		
		return seek(evader.getPos().plus(evader.getVel().scaleBy(lookAheadTime)));
	}
	
	/**  */
	public Vector2D evade(SmartActor pursuer) {
		Vector2D toPursuer = pursuer.getPos().minus(vehicle.getPos());
		
		double lookAheadTime = toPursuer.length() / (vehicle.getMaxSpeed() 
			+ pursuer.getMaxSpeed());
		
		return flee(pursuer.getPos().plus(pursuer.getVel().scaleBy(lookAheadTime)));
	}
	
	public Vector2D wander() {
		// add a small random vector to the targets position. Notice range -1 to 1.
		wanderTarget = wanderTarget.plus(new Vector2D.Double(MagicUtils.randInRange(-1, 1) * wanderJitter,
													  MagicUtils.randInRange(-1,1) * wanderJitter));
		
		// reproject unto unit circle						  
		wanderTarget.normalize();
		
		// increase the length of the vector to the same as the radius of the inner circle
		wanderTarget.scale(wanderRadius);
		
		// move target wander dist infront of agent
		Vector2D targetLocal = wanderTarget.plus(new Vector2D.Double(wanderDistance, 0));
		
		// project the target into world space
		Vector2D targetWorld = toWorldSpace(targetLocal, vehicle.getHeading(), vehicle.getSide());
		
		// steer towards it
		return targetWorld.minus(vehicle.getPos());
	}
	
	public Vector2D toWorldSpace(Vector2D localTarget,
									  Vector2D heading,
									  Vector2D side){
		matrix.rotate(heading.getX(), heading.getY(), side.getX(), side.getY());
		
		Point tPoint = new Point((int)localTarget.getX(), (int)localTarget.getY());
		matrix.transform(tPoint, tPoint);
		localTarget.setX(tPoint.x);
		localTarget.setY(tPoint.y);
		return localTarget;
	}	
		
	public Vector2D interpose(SmartActor agentA, SmartActor agentB){ 
		Vector2D midPoint = agentA.getPos().plus(agentB.getPos()).scaleBy(0.2);
		double timeToReachMidPoint = Vector2D.distance(vehicle.getPos(), midPoint)/vehicle.getMaxSpeed();
		
		Vector2D aPos = agentA.getPos().plus(agentA.getVel().scaleBy(timeToReachMidPoint));
		Vector2D bPos = agentB.getPos().plus(agentB.getVel().scaleBy(timeToReachMidPoint));
		
		midPoint = (aPos.plus(bPos)).scaleBy(0.2);
		return arrive(midPoint, DECEL_FAST);
	}
	
	public Vector2D getHidingPosition(Vector2D posOb, double radiusOb, Vector2D posTarget){
		double distanceFromBoundary = 30.0;
		double distanceAway = radiusOb + distanceFromBoundary;
		
		Vector2D toOb = posOb.minus(posTarget);
		toOb.normalize();
		return toOb.scaleBy(distanceAway).plus(posOb);
	}
	
	public Vector2D hide(SmartActor target, Vector<Actor2D> obstacles){
		double distanceToClosest = Integer.MAX_VALUE;
		Vector2D bestHidingSpot;
		
		int i = 0;
		Actor2D currOb = obstacles.get(i);
		bestHidingSpot = Vector2D.ZERO_VECTOR;
		
		while(currOb != obstacles.lastElement())
		{
			Vector2D hidingSpot = getHidingPosition(currOb.getPos(), currOb.getWidth(), target.getPos());
			
			double dist = Vector2D.distance(hidingSpot, vehicle.getPos());
			
			if(dist < distanceToClosest){
				distanceToClosest = dist;
				bestHidingSpot = hidingSpot;
			}
			
			i++;
			currOb = obstacles.get(i);
		}
		
		if(distanceToClosest == Integer.MAX_VALUE){ return evade(target); }
		
		return arrive(bestHidingSpot, DECEL_FAST);
	}
	
	public Vector2D followPath(){
		if(Vector2D.distanceSq(path.currentWaypoint(), vehicle.getPos()) 
			< waypointSeekDistSq){
			path.setNextWaypoint();
		}
		
		if(!path.finished())
		{
			return seek(path.currentWaypoint());
		}
		
		else
		{
			return arrive(path.currentWaypoint(), DECEL_NORMAL);
		}
	}
	
	public Vector2D offsetPursuit(SmartActor leader, Vector2D offset){
		Vector2D worldOffsetPos = toWorldSpace(offset,
													leader.getHeading(),
													leader.getSide());
													
		Vector2D toOffset = worldOffsetPos.minus(vehicle.getPos());
		
		double lookAheadTime = toOffset.length() / (vehicle.getMaxSpeed() + leader.getMaxSpeed());
		
		return arrive(worldOffsetPos.plus(leader.getVel().scaleBy(lookAheadTime)), DECEL_FAST);
	}
	/*
	public Vector2D calculate(){
		switch (calculateType) {
    		case CAL_TYPE_PD: return calculateDithered(); break;
    		case CAL_TYPE_WTS: return calculateWTS(); break;
    		case CAL_TYPE_WTS_PRIORITY: return calculateWTSP(); break;
    		default : break;
		}
	}
	
	public Vector2D calculateDithered(){
		steeringForce = Vector2D.ZERO_VECTOR;
		
		final double wallAvoidProb = 0.9;
		final double obstacleAvoidProb = 0.9;
		final double separationProb = 0.8;
		final double alignmentProb = 0.5;
		final double cohesionProb = 0.5;
		final double wanderProb = 0.8;
		
		final double weightObstacleAvoidance = 10;
		final double weightWallAvoidance = 10;
		final double weightSeparation = 10;
		final double weightCohesion = 10;
		
		if( doWallAvoidance && (Math.random() > wallAvoidProb))
		{
			// notice avoidWalls(Collections vehicles)
			steeringForce = avoidWalls(vehicle.group.gamelet.getScene().getWalls());
			steeringForce.scale(weightWallAvoidance/wallAvoidProb);
			
			if(steeringForce != Vector2D.ZERO_VECTOR)
			{
				steeringForce.truncate(vehicle.getMaxForce());
				
				return steeringForce;
			}	
		}
		
		if(doObstacleAvoidance && (Math.random() > obstacleAvoidProb))
		{
			steeringForce = steeringForce.plus( ((Vector2D) obstacleAvoidance(vehicle.group.gamelet.getScene().getObstacles()))
				.scaleBy(weightObstacleAvoidance/obstacleAvoidProb) );
				
			if(steeringForce != Vector2D.ZERO_VECTOR)
			{
				steeringForce.truncate(vehicle.getMaxForce());
				return steeringForce;				
			}
		}
		
		if(doSeparation && (Math.random() > separationProb))
		{
			steeringForce = steeringForce.plus( ((Vector2D) separation(vehicle.group.gamelet.getScene().getActors()))
				.scaleBy(weightSeparation/separationProb) );
			
			if(steeringForce != Vector2D.ZERO_VECTOR)
			{
				steeringForce.truncate(vehicle.getMaxForce());
				return steeringForce;
			}
		}
		
		if(doCohesion && (Math.random() > cohesionProb))
		{
			steeringForce = steeringForce.plus( ((Vector2D) cohesion(vehicle.group.gamelet.getScene().getActors()))
				.scaleBy(weightCohesion/cohesionProb) ); 
		}
	}
	
	public Vector2D calculateWTS(){
		steeringForce = steeringForce.plus(wander());
		steeringForce = steeringForce.plus(wallAvoidance());
		steeringForce = steeringForce.plus(obstacleAvoidance());
		steeringForce = steeringForce.plus(separation());
		steeringForce = steeringForce.plus(cohesion());
		steeringForce = steeringForce.plus(alignment());
		return steeringForce.truncate(MAX_STEERING_FORCE);
	}
	
	public Vector2D calculateWTSP(){
		return calculateDithered(); // for  now
	}
	*/
	public void obstacleAvoidanceOn(){ doObstacleAvoidance = true; }
	public void obstacleAvoidanceOff() { doObstacleAvoidance = false; }
	
	public void separationOn(){ doSeparation = true; }
	public void separationOff(){ doSeparation = false; }
	
	public void wanderOn(){ doWander = true; }
	public void wanderOff(){ doWander = false; }
	
	public void alignmentOn(){ doAlignment = true; }
	public void alignmentOff(){ doAlignment = false; }
	
	public void cohesionOn(){ doCohesion = true; }
	public void cohesionOff(){ doCohesion = false; }
	
	public void wallAvoidanceOn(){ doWallAvoidance = true; }
	public void wallAvoidanceOff(){ doWallAvoidance = false; }
	
	
	
}
